package navegar;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.nxt.comm.*;

import java.io.*;

import javax.bluetooth.*;

import lejos.util.*;

public class maestro extends thread {
	private static UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S2);
	static DifferentialPilot pilot = new DifferentialPilot(4f, 12f, Motor.A,
			Motor.C);
	static public DataOutputStream dos1;
	static public DataInputStream dis1;
	static int distMinima = 15;
	static int distSensada;
	static public BTConnection btc;
	static public RemoteDevice btrd;
	static public maestro maestro;
	static int veces = 0;
	static float obstaculo = 0;
	static float recibido;

	public static void main(String[] args) throws Exception {
		maestro = new maestro();

		LCD.drawString("INICIANDO", 0, 2);
		LCD.drawString("PRESIONE ENTER", 0, 4);
		Button.ENTER.waitForPress();
		LCD.clear();

		String pNombreDispositivoEsclavo = "NXT";
		LCD.drawString("Conectando...", 0, 0);
		btrd = Bluetooth.getKnownDevice(pNombreDispositivoEsclavo);

		if (btrd == null) {
			LCD.clear();
			LCD.drawString("No encontro dispositivo", 0, 0);
			Button.waitForAnyPress();
			System.exit(1);
		}
		btc = Bluetooth.connect(btrd);

		if (btc == null) {
			LCD.clear();
			LCD.drawString("Connect fail", 0, 0);
			Button.waitForAnyPress();
			System.exit(1);
		}

		LCD.clear();
		LCD.drawString("Connected", 0, 0);

		dis1 = btc.openDataInputStream();
		dos1 = btc.openDataOutputStream();

		while (veces < 4) {

			pilot.forward();
			maestro.run();
				

		}

	};

	public void run() {
		// TODO Auto-generated method stub

		distSensada = sensor.getDistance();
		LCD.drawInt(distSensada, 4, 0, 3);

		if (distSensada < distMinima) {
			try {
				dos1.writeFloat(veces);
				dos1.flush();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			maestro.evasion();
			veces = veces + 1;
		}
	}

	public void evasion() {

		LCD.drawString("'evadiendo'", 0, 7);
		Delay.msDelay(1000);
		pilot.travel(-30);
		pilot.rotate(25);
		LCD.drawString("'obstaculo'", 0, 8);
		Delay.msDelay(1000);
		LCD.clear();
	}

	
}
